#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <string.h>
#include <time.h>

#define TOTP_LENGTH 8       //TOTP长度
#define TOTP_BASIC_LENGTH 5 //basic长度
#define STEP 60             //一个步长单位(精确到秒)：60秒
#define EXTEND_STEP 10      //容忍扩展步长10次（正负）
#define MAX_MESSAGE_LENGTH 4096

/**
 * 
 * 原始公式:TOTP =Truncate(HMACSHA(K, T);<br/>
 * 现有公式:TOTP =Mix(Truncate(HMACSHA((CODE+SECRET_KEY),(Now-T0-N*STEP)/STEP)),RK);
 * <br/>
 * <br/>
 * 解释：<br/>
 * T=(Now-T0-N*STEP)/STEP) ：是步长，表示当前时间-初始时间后除去步长单位，N为容忍步长个数。<br/>
 * K=(CODE+SECRET_KEY) : 密钥K为自有密钥code和公共密钥SECRET_KEY拼接，code可以是任意约定。<br/>
 * RK : 密钥加盐随机数，用于创建不重复的totp;<br/>
 * HMACSHA ：一种基础信息摘要算法，加密方式可以是：HmacSHA1，HmacSHA256，HmacSHA512 .. <br/>
 * Truncate : 一种截断函数，用于截取CODE_DIGITS位长度totp; <br/>
 * Mix : 一种字符混淆函数，用户将随机数RK插入到以生成的totp中。<br/>
 * <br/>
 * 专利：<br/>
 * step1:,一种防止重复TOTP密码重复的方案；RAND_DIGITS<br/>
 * step2:,一种在时钟不严格同步的情况下TOTP优化；N=EXTEND_STEP<br/>
 * 
 * @author yuanqiyong@unovo.com.cn
 * @date 2019年7月5日
 * @company http://www.lianyuplus.com/
 */
/****************************************/
/* sha1()                               */
/* Performs the NIST SHA-1 algorithm    */
/****************************************/
unsigned long int ft(
    int t,
    unsigned long int x,
    unsigned long int y,
    unsigned long int z)
{
    unsigned long int a, b, c;

    if (t < 20)
    {
        a = x & y;
        b = (~x) & z;
        c = a ^ b;
    }
    else if (t < 40)
    {
        c = x ^ y ^ z;
    }
    else if (t < 60)
    {
        a = x & y;
        b = a ^ (x & z);
        c = b ^ (y & z);
    }
    else if (t < 80)
    {
        c = (x ^ y) ^ z;
    }

    return c;
}

unsigned long int k(int t)
{
    unsigned long int c;

    if (t < 20)
    {
        c = 0x5a827999;
    }
    else if (t < 40)
    {
        c = 0x6ed9eba1;
    }
    else if (t < 60)
    {
        c = 0x8f1bbcdc;
    }
    else if (t < 80)
    {
        c = 0xca62c1d6;
    }

    return c;
}

unsigned long int rotr(int bits, unsigned long int a)
{
    unsigned long int c, d, e, f, g;
    c = (0x0001 << bits) - 1;
    d = ~c;

    e = (a & d) >> bits;
    f = (a & c) << (32 - bits);

    g = e | f;

    return (g & 0xffffffff);
}

unsigned long int rotl(int bits, unsigned long int a)
{
    unsigned long int c, d, e, f, g;
    c = (0x0001 << (32 - bits)) - 1;
    d = ~c;

    e = (a & c) << bits;
    f = (a & d) >> (32 - bits);

    g = e | f;

    return (g & 0xffffffff);
}

void sha1(
    unsigned char *message,
    int message_length,
    unsigned char *digest)
{
    int i;
    int num_blocks;
    int block_remainder;
    int padded_length;

    unsigned long int l;
    unsigned long int t;
    unsigned long int h[5];
    unsigned long int a, b, c, d, e;
    unsigned long int w[80];
    unsigned long int temp;
    /* Calculate the number of 512 bit blocks */
    padded_length = message_length + 8; /* Add length for l */
    padded_length = padded_length + 1;  /* Add the 0x01 bit postfix */

    l = message_length * 8;

    num_blocks = padded_length / 64;
    block_remainder = padded_length % 64;

    if (block_remainder > 0)
    {
        num_blocks++;
    }
    padded_length = padded_length + (64 - block_remainder);

    /* clear the padding field */
    for (i = message_length; i < (num_blocks * 64); i++)
    {
        message[i] = 0x00;
    }
    /* insert b1 padding bit */
    message[message_length] = 0x80;
    /* Insert l */
    message[(num_blocks * 64) - 1] = (unsigned char)(l & 0xff);
    message[(num_blocks * 64) - 2] = (unsigned char)((l >> 8) & 0xff);
    message[(num_blocks * 64) - 3] = (unsigned char)((l >> 16) & 0xff);
    message[(num_blocks * 64) - 4] = (unsigned char)((l >> 24) & 0xff);

    /* Set initial hash state */
    h[0] = 0x67452301;
    h[1] = 0xefcdab89;
    h[2] = 0x98badcfe;
    h[3] = 0x10325476;
    h[4] = 0xc3d2e1f0;

    for (i = 0; i < num_blocks; i++)
    {
        /* Prepare the message schedule */
        for (t = 0; t < 80; t++)
        {
            if (t < 16)
            {
                w[t] = (256 * 256 * 256) * message[(i * 64) + (t * 4)];
                w[t] += (256 * 256) * message[(i * 64) + (t * 4) + 1];
                w[t] += (256) * message[(i * 64) + (t * 4) + 2];
                w[t] += message[(i * 64) + (t * 4) + 3];
            }
            else if (t < 80)
            {
                w[t] = rotl(1, (w[t - 3] ^ w[t - 8] ^ w[t - 14] ^ w[t - 16]));
            }
        }
        /* Initialize the five working variables */
        a = h[0];
        b = h[1];
        c = h[2];
        d = h[3];
        e = h[4];
        /* iterate a-e 80 times */
        for (t = 0; t < 80; t++)
        {
            temp = (rotl(5, a) + ft(t, b, c, d)) & 0xffffffff;
            temp = (temp + e) & 0xffffffff;
            temp = (temp + k(t)) & 0xffffffff;
            temp = (temp + w[t]) & 0xffffffff;
            e = d;
            d = c;
            c = rotl(30, b);
            b = a;
            a = temp;
        }
        /* compute the ith intermediate hash value */
        h[0] = (a + h[0]) & 0xffffffff;
        h[1] = (b + h[1]) & 0xffffffff;
        h[2] = (c + h[2]) & 0xffffffff;
        h[3] = (d + h[3]) & 0xffffffff;
        h[4] = (e + h[4]) & 0xffffffff;
    }

    digest[3] = (unsigned char)(h[0] & 0xff);
    digest[2] = (unsigned char)((h[0] >> 8) & 0xff);
    digest[1] = (unsigned char)((h[0] >> 16) & 0xff);
    digest[0] = (unsigned char)((h[0] >> 24) & 0xff);

    digest[7] = (unsigned char)(h[1] & 0xff);
    digest[6] = (unsigned char)((h[1] >> 8) & 0xff);
    digest[5] = (unsigned char)((h[1] >> 16) & 0xff);
    digest[4] = (unsigned char)((h[1] >> 24) & 0xff);

    digest[11] = (unsigned char)(h[2] & 0xff);
    digest[10] = (unsigned char)((h[2] >> 8) & 0xff);
    digest[9] = (unsigned char)((h[2] >> 16) & 0xff);
    digest[8] = (unsigned char)((h[2] >> 24) & 0xff);

    digest[15] = (unsigned char)(h[3] & 0xff);
    digest[14] = (unsigned char)((h[3] >> 8) & 0xff);
    digest[13] = (unsigned char)((h[3] >> 16) & 0xff);
    digest[12] = (unsigned char)((h[3] >> 24) & 0xff);

    digest[19] = (unsigned char)(h[4] & 0xff);
    digest[18] = (unsigned char)((h[4] >> 8) & 0xff);
    digest[17] = (unsigned char)((h[4] >> 16) & 0xff);
    digest[16] = (unsigned char)((h[4] >> 24) & 0xff);
}

/******************************************************/
/* hmac-sha1()                                        */
/* Performs the hmac-sha1 keyed secure hash algorithm */
/******************************************************/
void hmac_sha1(
    unsigned char *key,
    int key_length,
    unsigned char *data,
    int data_length,
    unsigned char *digest)
{
    int b = 64; /* blocksize */
    unsigned char ipad = 0x36;
    unsigned char opad = 0x5c;

    unsigned char k0[64];
    unsigned char k0xorIpad[64];
    unsigned char step7data[64];
    unsigned char step5data[MAX_MESSAGE_LENGTH + 128];
    unsigned char step8data[64 + 20];
    int i;

    for (i = 0; i < 64; i++)
    {
        k0[i] = 0x00;
    }
    if (key_length != b) /* Step 1 */
    {
        /* Step 2 */
        if (key_length > b)
        {
            sha1(key, key_length, digest);
            for (i = 0; i < 20; i++)
            {
                k0[i] = digest[i];
            }
        }
        else if (key_length < b) /* Step 3 */
        {
            for (i = 0; i < key_length; i++)
            {
                k0[i] = key[i];
            }
        }
    }
    else
    {
        for (i = 0; i < b; i++)
        {
            k0[i] = key[i];
        }
    }
    /* Step 4 */
    for (i = 0; i < 64; i++)
    {
        k0xorIpad[i] = k0[i] ^ ipad;
    }
    /* Step 5 */
    for (i = 0; i < 64; i++)
    {
        step5data[i] = k0xorIpad[i];
    }
    for (i = 0; i < data_length; i++)
    {
        step5data[i + 64] = data[i];
    }
    /* Step 6 */
    sha1(step5data, data_length + b, digest);

    /* Step 7 */
    for (i = 0; i < 64; i++)
    {
        step7data[i] = k0[i] ^ opad;
    }
    /* Step 8 */
    for (i = 0; i < 64; i++)
    {
        step8data[i] = step7data[i];
    }
    for (i = 0; i < 20; i++)
    {
        step8data[i + 64] = digest[i];
    }
    /* Step 9 */
    sha1(step8data, b + 20, digest);
}
/******************************************************/
/* truncate                                           */
/******************************************************/
int truncate(unsigned char digest[])
{
    int offset = digest[19] & 0xf;
    int res = (((digest[offset + 0] & 0x7f) << 24) |
               ((digest[offset + 1] & 0xff) << 16) |
               ((digest[offset + 2] & 0xff) << 8) |
               (digest[offset + 3] & 0xff));
    int x = 10, y = 5;
    int total = pow(x, y);
    return res % total;
}
/******************************************************/
/* Mix                                                */
/******************************************************/

void longToStr(long num, unsigned char *str, int size) //左边补零
{
    char index[] = "0123456789ABCDEFGHIJKLMNOPQRSTUVWXYZ"; //索引表
    int i = size, radix = 10;
    while (num > 0)
    {
        str[--i] = index[num % (unsigned)radix]; //ascii
        num /= radix;                            //unum去掉最后一位
    };                                           //直至unum为0退出循环
    for (size_t i = 0; i < size; i++)
        if (str[i] < 48 || str[i] > 57)
            str[i] = 48;
    str[size] = '\0'; //在字符串最后添加'\0'字符，c语言字符串以'\0'结束。
}
// 生成下标随机数[不重复] 和 占位随机数[可重复]
void getRandomTwo(unsigned char *indexs, unsigned char *rands)
{
    int size = TOTP_LENGTH - TOTP_BASIC_LENGTH;
    srand((unsigned)time(NULL));
    for (size_t i = 0; i < size; i++)
    {
        int t = rand() % TOTP_LENGTH;
        int bo = 0;
        for (size_t x = 0; x < i; x++)
        {
            if (indexs[x] == t)
            {
                bo = 1;
                i--;
            }
        }
        if (bo == 0)
            indexs[i] = t;
    }
    for (size_t j = 0; j < size; j++)
    {
        int t = rand() % 9;
        rands[j] = t;
    }
}

int equalsTotp(int totp5, unsigned char *totp_target)
{
    int L5 = TOTP_BASIC_LENGTH, L8 = TOTP_LENGTH;
    unsigned char c5[L5];
    longToStr(totp5, c5, L5);        // 5位
    unsigned char *c8 = totp_target; // 8位
    int j = 0;                       //初始匹配下标
    for (int i = 0; i < L8; i++)
        if (j < L5 && c8[i] == c5[j]) // 必须先判断大小
            j++;                      //匹配下标+1
    return j == L5 ? 1 : 0;           // 顺序递增直到结束
}

int buildBasic(unsigned char *secret_key, long timeNow)
{
    long now = timeNow / STEP;
    unsigned char times[10];
    unsigned char digest[20];
    longToStr(now, times, 10);
    hmac_sha1(secret_key, strlen(secret_key), times, strlen(times), digest); //hmac_sha1
    return truncate(digest);                                                 //截断dd
}
void mix(int totp, unsigned char *alltotp)
{
    // step1: 一种防止重复TOTP密码重复的方案
    int size = TOTP_LENGTH - TOTP_BASIC_LENGTH;
    unsigned char totps[TOTP_BASIC_LENGTH];
    longToStr(totp, totps, TOTP_BASIC_LENGTH);
    unsigned char indexs[size], rands[size];
    getRandomTwo(indexs, rands);
    //==DEBUG=====================
    //     printf("\ntotp_basic=");
    //     for (size_t i = 0; i < TOTP_BASIC_LENGTH; i++)
    //         printf("%c", totps[i]);
    //     printf("\nrands=");
    //     for (size_t i = 0; i < size; i++)
    //         printf("%d", rands[i]);
    //     printf("\nindexs=");
    //     for (size_t i = 0; i < size; i++)
    //         printf("%d", indexs[i]);
    int j = 0;
    for (size_t i = 0; i < TOTP_LENGTH; i++)
    {
        int bo = 0;
        for (size_t x = 0; x < size; x++)
        {
            if (indexs[x] == i)
            {
                alltotp[i] = rands[x] + 48;
                bo = 1;
                break;
            }
        }
        if (bo == 0 && j < TOTP_BASIC_LENGTH)
            alltotp[i] = totps[j++];
    }
}
int unmix(unsigned char *secret_key, unsigned char *totp_target)
{
    // step2:一种在时钟不严格同步的情况下TOTP优化
    for (int n = 0; n < EXTEND_STEP * 2; n++) // 正负延申 EXTEND_STEP * STEP个步长时间；(从历史时间到未来时间)
        if (equalsTotp(buildBasic(secret_key, time(NULL) - EXTEND_STEP * STEP + n * STEP), totp_target) == 1)
            return 1; //比对成功
    return 0;
}

/******************************************************/
/* 对外接口                                            */
/******************************************************/
/**
 * 生成8位TOTP密码
 * [入参]secret_key 门锁密钥key
 * [出参]totp_digest 8位密码
 */
void buildTOTP(unsigned char *secret_key, unsigned char *totp_digest)
{
    long timeNow = time(NULL);
    int basic = buildBasic(secret_key, timeNow); //创建基本位
    mix(basic, totp_digest);                     //混淆
}
/**
 * 校验8位TOTP密码
 * [入参]secret_key 门锁密钥key
 * [入参]totp_digest  8位密码
 * [出参] int 1=校验通过，0=校验不通过
 */
int verifyTOTP(unsigned char *secret_key, unsigned char *totp_digest)
{
    return unmix(secret_key, totp_digest);
}

/******************************************************/
/* main                                               */
/******************************************************/
int main()
{
    unsigned char secret_key[] = "lianyu@12345678901234567890"; // // 门锁密钥[每个门锁的密钥都不一样]
    //== 生成TOTP ==============================================================
    unsigned char TOTP[TOTP_LENGTH]; // 生成8位TOTP
    buildTOTP(secret_key, TOTP);
    printf("\n生成TOTP:");
    for (size_t i = 0; i < TOTP_LENGTH; i++)
        printf("%c", TOTP[i]);

    //== 校验TOTP ==============================================================
    // unsigned char totp_target[] = "07236791"; //用户录入8位TOTP;
    unsigned char *totp_target = TOTP; //用户录入8位TOTP;
    int bo = verifyTOTP(secret_key, totp_target);
    printf("\n校验结果：%s", (bo == 1 ? "成功" : "失败"));
}